#ifndef __DPL_LOCKFREE_QUEUE_H__ 
#define __DPL_LOCKFREE_QUEUE_H__

#include <cstdint>
#include <chrono>
#include <atomic>
#include <thread>
#include <functional>

namespace dpl {

enum State {
    Empty = 0,
    Full  = 1,
};

template<typename T>
class LockfreeQueue
{

    struct Node {
        T data_;
        std::atomic<int> sta_;

        Node(): sta_(State::Empty) {};
        ~Node() {};
    };

    Node* queue_;
    std::atomic<uint32_t> front_;
    std::atomic<uint32_t> rear_;
    std::atomic<uint32_t> max_size_; 

    std::atomic<int> size_;
    std::atomic<int> writeable_;

    std::function<void(T&)> delete_func_;

public:
    LockfreeQueue(uint32_t max): front_(0), rear_(0), size_(0), writeable_(max), max_size_(max), delete_func_(NULL)
    {
        queue_ = new Node[max];
    }

    LockfreeQueue(uint32_t max, std::function<void(T&)> f): front_(0), rear_(0), size_(0), writeable_(max), max_size_(max)
    {
        queue_ = new Node[max];
        delete_func_ = f;
    }

    ~LockfreeQueue() 
    {
        Clear();
        delete []queue_;
    }

    bool Empty() 
    {
        return size_ == 0;
    }

    int Size()
    {
        return size_;
    }

    void Clear()
    {
        T data;

        while(size_) {
            if(Pop(data) && delete_func_) {
                delete_func_(data);
            }
        }
    }

    bool Push(T data)
    {
        if(writeable_.fetch_sub(1, std::memory_order_seq_cst) <= 0) {
            writeable_.fetch_add(1, std::memory_order_seq_cst);
            return false;
        }

        uint32_t pos = rear_.fetch_add(1, std::memory_order_seq_cst) % max_size_;

        int node_empty = State::Empty;
        while(!queue_[pos].sta_.compare_exchange_strong(node_empty, State::Full)) {
            node_empty = State::Empty;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }

        queue_[pos].data_ = data;
        size_.fetch_add(1, std::memory_order_seq_cst);

        return true;
    }

    template <typename Rep, typename Period>
    bool TimePush(T data, std::chrono::duration<Rep, Period> wait_dur)
    {
        auto deadline = std::chrono::steady_clock::now() + wait_dur;

        uint32_t pos = rear_.fetch_add(1, std::memory_order_seq_cst) % max_size_;

        int node_empty = State::Empty;
        while(!queue_[pos].sta_.compare_exchange_strong(node_empty, State::Full)) {
            if(std::chrono::steady_clock::now() >= deadline)
                return false;

            node_empty = State::Empty;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }

        queue_[pos].data_ = data;
        size_.fetch_add(1, std::memory_order_seq_cst);
        writeable_.fetch_sub(1, std::memory_order_seq_cst);

        return true;
    }

    bool TryPush(T data)
    {
        if(writeable_.fetch_sub(1, std::memory_order_seq_cst) <= 0) {
            writeable_.fetch_add(1, std::memory_order_seq_cst);
            return false;
        }

        uint32_t pos = rear_.fetch_add(1, std::memory_order_seq_cst) % max_size_;

        int node_empty = State::Empty;
        if(!queue_[pos].sta_.compare_exchange_strong(node_empty, State::Full)) {
            return false;
        }

        queue_[pos].data_ = data;
        size_.fetch_add(1, std::memory_order_seq_cst);

        return true;
    }

    bool Pop(T &data) 
    {
        uint32_t pos;
        int node_full = State::Full;

        while(true) {
            if(size_ == 0) return false;
            
            pos = front_ % max_size_;
            data = queue_[pos].data_;
            node_full = State::Full;

            if(queue_[pos].sta_.compare_exchange_strong(node_full, State::Empty)) 
                break;

            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }

        front_.fetch_add(1, std::memory_order_seq_cst);

        size_.fetch_sub(1, std::memory_order_seq_cst);
        writeable_.fetch_add(1, std::memory_order_seq_cst);
        
        return true;
    } 

    template <typename Rep, typename Period>
    bool TimePop(T &data, std::chrono::duration<Rep, Period> wait_dur)
    {
        uint32_t pos;
        auto deadline = std::chrono::steady_clock::now() + wait_dur;
        int node_full = State::Full;

        while(true) {
            pos = front_ % max_size_;
            data = queue_[pos].data_;
            node_full = State::Full;

            if(queue_[pos].sta_.compare_exchange_strong(node_full, State::Empty))
                break;

            if(std::chrono::steady_clock::now() >= deadline)
                return false;

            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }

        front_.fetch_add(1, std::memory_order_seq_cst);

        size_.fetch_sub(1, std::memory_order_seq_cst);
        writeable_.fetch_add(1, std::memory_order_seq_cst);

        return true;
    }

    bool TryPop(T &data)
    {
        uint32_t pos;
        int node_full = State::Full;

        if(size_ == 0) return false;

        pos = front_ % max_size_;
        data = queue_[pos].data_;

        if(!queue_[pos].sta_.compare_exchange_strong(node_full, State::Empty))
            return false;

        front_.fetch_add(1, std::memory_order_seq_cst);

        size_.fetch_sub(1, std::memory_order_seq_cst);
        writeable_.fetch_add(1, std::memory_order_seq_cst);

        return true;
    }
};

} // dpl

#endif // __DPL_LOCKFREE_QUEUE_H__
